#include "MyProject.h"

#include "AS5600.h"

/************************************************
本程序仅供学习，引用代码请标明出处
使用教程：https://blog.csdn.net/loop222/article/details/120471390
创建日期：20210925
作    者：loop222 @郑州
************************************************/
/******************************************************************************/
long  cpr;
float full_rotation_offset;
long  angle_data_prev;
unsigned long velocity_calc_timestamp;
float angle_prev;
/******************************************************************************/

/******************************************************************************/
void MagneticSensor_Init(void)
{
	bsp_as5600Init();
	
	full_rotation_offset = 0;
	velocity_calc_timestamp=0;
}
/******************************************************************************/
float getAngle(void)		//角度值
{
		return  bsp_as5600GetAngle();
}
/******************************************************************************/
// Shaft velocity calculation轴速度计算
float getVelocity(void)
{
	unsigned long now_us;
	float Ts, angle_c, vel;

	// calculate sample time 计算两次时间间隔
	now_us = __HAL_TIM_GET_COUNTER(&htim2); //
	if(now_us<velocity_calc_timestamp) 	Ts = (float)(UINT32_MAX - velocity_calc_timestamp + now_us) * 1e-6;	//  单位S
	else																Ts = (float)(now_us - velocity_calc_timestamp) * 1e-6;	
	velocity_calc_timestamp=now_us;  //save timestamp for next call为下次调用保存时间戳
  // quick fix for strange cases (micros overflow)对奇怪情况(微溢出)的快速修复
  if(Ts == 0 || Ts > 0.5)
	{
		printf_uart2("Ts error:%.3f\n",Ts);
		Ts = 1e-3; 
	}
	
	
	// current angle
	angle_c = getAngle();
	// velocity calculation
	vel = (angle_c - angle_prev)/Ts;

	// save variables for future pass
	angle_prev = angle_c;
	velocity_calc_timestamp = now_us;
	return vel;
}
/******************************************************************************/



